/**
 * @file CONTOUR
 * @brief Contour detector and homography
 * @author M. Farra
 */

#include <stdio.h>
#include <time.h>
#include <iostream>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc/imgproc.hpp"

using namespace cv;

void readme();
vector<Point> getBestContour(const Mat &);
vector<Point2f> getFloatHull(const vector<Point> &, const Size, const Rect);

/**
 * @function vto2f
 */
void vto2f(const vector<Point> in, vector<Point2f> & out){
	out.clear();
	out.resize(in.size());

	for (int i=0; i<in.size(); i++)
		out[i] = in[i];
}

//ENUM for tests
enum test {
	FIRST,
	FOUR,
	SIX,
	TEN,
	FOURTEEN
};

int current_test = FOUR;


const char* keys = 
{
  "{c |camera   |false    | use camera or not}"
  "{fn|file_name|/Users/Apple/Documents/Work/AUB/Courses/fyp/fourth_experiment/t43-touch.mov | movie file}"
  "{fn2|file_name2|/Users/Apple/Documents/Work/AUB/Courses/fyp/fourth_experiment/t44-touch.mov | movie file}"

};


//------------------------------------------------------------------------------------------------------
// DEFINE GLOBAL HOMOGRAPHY MATRICES
//------------------------------------------------------------------------------------------------------

Mat h_right(3,3,CV_64F);
Mat h_left(3,3,CV_64F);
Rect b_right, b_left;

float rhx, rhy;
float lhx, lhy;
float minrx, minry;
float minlx, minly;

Point2f o_r;
Point2f o_l;
/**
* @function Init homography
* @brief Initializes the homography matrix according to the test case
* @returns The right and left homography matrices, the left and Right 
*/
void init_homography(int curr)	 
{
	float change = 0.0;

	//Right	
	switch(curr)
	{

		case FOUR:
		{
			double temp_right[3][3] =   
			{
				{15.247969, -24.742434, 293.90598},
				{1.4445137, 11.005177, 185.91911},
				{-0.026479064, -0.029542347, 25}
			};
			Mat t_r = Mat(3,3,CV_64F, temp_right);
			t_r.copyTo(h_right);

			vector<Point2f> mask_right; 

			Point p1 = Point2f(293.906,185.919); mask_right.push_back(p1);
			Point p2 = Point2f(527.667,250.55); mask_right.push_back(p2);
			Point p3 = Point2f(199.69,282.704); mask_right.push_back(p3);
			Point p4 = Point2f(456.203,394.252); mask_right.push_back(p4);
			b_right = boundingRect(mask_right); // Right boundary

			vector<Point2f> temp_out;
			perspectiveTransform(mask_right, temp_out, h_right.inv());

			float max_x = 0; float min_x = 999;
			float max_y = 0; float min_y = 999;
			for (int i=0; i<temp_out.size(); i++)
			{
				std::cout<<temp_out[i]<<std::endl;

				if (temp_out[i].x > max_x)
				{
					max_x = temp_out[i].x;
				}
				if (temp_out[i].x < min_x)
				{
					min_x = temp_out[i].x;
				}
				if (temp_out[i].y > max_y)
				{
					max_y = temp_out[i].y;
				}
				if (temp_out[i].y < min_y)
				{
					min_y = temp_out[i].y;
				}
			}
			minrx = min_x; minry = min_y;
			std::cout<<"MIN-MAX R: "<<min_x<<", "<<min_y<<", "<<max_x<<", "<<max_y<<std::endl;
			rhx = max_x - min_x; rhy = max_y - min_y;
			std::cout<<"MIN H: "<<rhx<<", "<<rhy<<std::endl;
			break;
		}
	}
	//Left
	switch(curr)
	{
		case FOUR:
		{
			double temp_left[3][3] =   
			{
				{35.419289, 4.1167402, 216.50978},
				{-7.001616, 8.3707304, 258.32034},
				{0.018114273, -0.041160546, 25}
			};
			Mat t_l = Mat(3,3,CV_64F, temp_left);
			t_l.copyTo(h_left);

			vector<Point2f> mask_right; 

			Point p1 = Point2f(216.51,258.32); mask_right.push_back(p1);
			Point p2 = Point2f(436.595,176.701); mask_right.push_back(p2);
			Point p3 = Point2f(298.532,377.959); mask_right.push_back(p3);
			Point p4 = Point2f(554.192,259.992); mask_right.push_back(p4);
			b_left = boundingRect(mask_right); // Right boundary

			vector<Point2f> temp_out;
			perspectiveTransform(mask_right, temp_out, h_left.inv());

			float max_x = 0; float min_x = 999;
			float max_y = 0; float min_y = 999;
			for (int i=0; i<temp_out.size(); i++)
			{
				std::cout<<temp_out[i]<<std::endl;
				if (temp_out[i].x > max_x)
				{
					max_x = temp_out[i].x;
				}
				if (temp_out[i].x < min_x)
				{
					min_x = temp_out[i].x;
				}
				if (temp_out[i].y > max_y)
				{
					max_y = temp_out[i].y;
				}
				if (temp_out[i].y < min_y)
				{
					min_y = temp_out[i].y;
				}
			}
			minly = min_y; minlx = min_x;
			std::cout<<"MIN-MAX L: "<<min_x<<", "<<min_y<<", "<<max_x<<", "<<max_y<<std::endl;
			lhx = max_x - min_x; lhy = max_y - min_y;
			std::cout<<"MIN H: "<<lhx<<", "<<lhy<<std::endl;
			break;
		}
	}

}


// Benchmarking constants
time_t start, end;
double fps; int counter = 0; double sec;


/**
 * @function main
 * @brief Main function
 */
int main( int argc, char** argv )
{

	init_homography(current_test);
//------------------------------------------------------------------------------------------------------
// INITIALIZE VIDEO
//------------------------------------------------------------------------------------------------------

  CommandLineParser parser(argc, argv, keys);
  bool useCamera = parser.get<bool>("camera");
  string file = parser.get<string>("file_name");
  string file2 = parser.get<string>("file_name2");
  VideoCapture cap;
  VideoCapture cap2;

	if( useCamera ) {
	  cap.open(0);
	}
	else {
	  cap.open(file.c_str());
	  cap2.open(file2.c_str());
	}

 // parser.printParams();

  if( !cap.isOpened() || !cap2.isOpened() ) {
	printf("can not open camera or video file\n");
	return -1;
  }

//------------------------------------------------------------------------------------------------------
// CONTOUR DETECTION ON TWO VIDEOS
//------------------------------------------------------------------------------------------------------

	Mat left_frame, right_frame; // Frames after boundary crop
	Mat raw_left_frame, raw_right_frame; // Input from the cameras

	cap>>raw_left_frame;
	cap2>>raw_right_frame;

	// Use the masks to crop the frames;
	left_frame = raw_left_frame(b_left);
	right_frame = raw_right_frame(b_right);

	Mat prev_left_gray, curr_left_gray, prev_right_gray, curr_right_gray; // Gray scale images
	Mat img_object, img_scene; // Final images
	Mat left_bit, right_bit; // Binary masks

	// Initialize prev and curr to the same grayscale frame
	cvtColor(left_frame, curr_left_gray, CV_RGB2GRAY);
	prev_left_gray.create(curr_left_gray.size(), curr_left_gray.type());
	img_object.create(left_frame.size(), left_frame.type());
	curr_left_gray.copyTo(prev_left_gray);

	cvtColor(right_frame, curr_right_gray, CV_RGB2GRAY);
	prev_right_gray.create(curr_right_gray.size(), curr_right_gray.type());
	img_scene.create(right_frame.size(), right_frame.type());
	curr_right_gray.copyTo(prev_right_gray);

	time(&start);
	for (;;) 
	{

		img_object = Scalar(0);
		img_scene = Scalar(0);

		cap>>raw_left_frame;
		cap2>>raw_right_frame;

		// Use the masks to crop the frames;
		left_frame = raw_left_frame(b_left);
		right_frame = raw_right_frame(b_right);

		// Change to grayscale
		cvtColor(left_frame, curr_left_gray, CV_RGB2GRAY);
		cvtColor(right_frame, curr_right_gray, CV_RGB2GRAY);

		// Absolute difference between reference and current frame
		absdiff(curr_left_gray,prev_left_gray, left_bit);
		threshold(left_bit,left_bit,60,255,CV_THRESH_BINARY);
		left_frame.copyTo(img_object, left_bit);

		absdiff(curr_right_gray,prev_right_gray, right_bit);
		threshold(right_bit,right_bit,60,255,CV_THRESH_BINARY);
		right_frame.copyTo(img_scene, right_bit);

		// Get contour of components
		vector<Point> contour_right = getBestContour(right_bit);
		vector<Point> contour_left = getBestContour(left_bit);
		vector<Point2f> right_hull = getFloatHull(contour_right, right_frame.size(), b_right);
		vector<Point2f> left_hull = getFloatHull(contour_left, left_frame.size(), b_left);

		// Circle the hull
		float rfx = right_frame.size().width;
		float rfy = right_frame.size().height;
		float lfx = left_frame.size().width;
		float lfy = left_frame.size().height;

		if (contour_right.size()>0) 
		{
			for (int i=0; i<right_hull.size(); i++) 
			{
				circle(right_frame, Point2f(right_hull[i].x, right_hull[i].y), 5, Scalar(255,0,0,255));
			}
		}
		if (contour_left.size()>0) 
		{
			for (int i=0; i<left_hull.size(); i++) 
			{
				circle(left_frame, Point2f(left_hull[i].x, left_hull[i].y), 5, Scalar(255,0,0,255));
			}
		}


//------------------------------------------------------------------------------------------------------
// HOMOGRAPHY
//------------------------------------------------------------------------------------------------------

		// Use perspective transform 
		Size tot = Size(500,500);

		Mat out_view = Mat(tot, CV_8UC3, Scalar(0));
		Mat right_view = Mat(tot, CV_8UC3, Scalar(0));
		Mat left_view = Mat(tot, CV_8UC3, Scalar(0));
		
		Mat rtotal = Mat(tot, right_frame.type(), Scalar(0));
		Mat ltotal = Mat(tot, right_frame.type(), Scalar(0));

		vector<Point2f> right_output;
		vector<Point2f> left_output;
		if (contour_right.size()>0)
		{
			perspectiveTransform(right_hull, right_output, h_right.inv());
			warpPerspective(raw_right_frame, rtotal, h_right.inv(), tot);
		}
		if (contour_left.size() > 0)
		{
			perspectiveTransform(left_hull, left_output, h_left.inv());
			warpPerspective(raw_left_frame, ltotal, h_left.inv(), tot);
		}

		// warpPerspective(rtotal,rtotal,h_right,right_frame.size());
		// warpPerspective(ltotal,ltotal,h_left,left_frame.size());
		
		imshow("RTOTAL", rtotal);
		imshow("LTOTAL", ltotal);

		for (int i=0; i<right_output.size(); i++)
		{
	//		std::cout<<"X:"<<right_output[i].x<<"Y:"<<right_output[i].y<<">>";
			right_output[i].y = (right_output[i].y - minry) * 500/rhy;
			right_output[i].x = (right_output[i].x - minrx) * 500/rhx;
			//std::cout<<"X:"<<right_output[i].x<<"Y:"<<right_output[i].y<<std::endl;
		}

		for (int i=0; i<left_output.size(); i++)
		{
			left_output[i].y = (left_output[i].y - minly) * 500/lhy;
			left_output[i].x = (left_output[i].x - minlx) * 500/lhx;
		//	std::cout<<"X:"<<right_output[i].x<<"Y:"<<right_output[i].y<<std::endl;
		}

		if (contour_right.size()>0 && contour_left.size()>0)
		{			
			float min_distance = 100;
			float distance;
			Point2f from_left, from_right;
			for (int i=0; i<right_output.size(); i++)
			{
				for (int j = 0; j<right_output.size();j++)
				{
					from_left = left_output[j];
					from_right = right_output[i];
		
					distance = sqrt(pow((from_left.y-from_right.y),2)+pow((from_left.x-from_right.x),2));
					if (distance<min_distance)
					{
						min_distance = distance;
					}
				}
			}
			// std::cout<<min_distance<<std::endl;
		
			if (distance<20)
			{
				std::cout<<"TOUCH!"<<std::endl;
			}
		}

	//	std::cout<<"Right output size: "<<right_output.size()<<std::endl;
	//	std::cout<<"Left output size: "<<left_output.size()<<std::endl;


		for (int i=0; i<right_output.size(); i++)
		{
			Point p = Point(right_output[i].x, right_output[i].y); //std::cout<<p<<std::endl;
			circle(out_view, p , 5, Scalar(0,0,255));
			circle(right_view, p, 5, Scalar(0,0,255));
		}
		for (int i=0; i<left_output.size(); i++)
		{
			Point p = Point(left_output[i].x, left_output[i].y);
			circle(out_view, p, 5, Scalar(0,255,0));
			circle(left_view, p, 5, Scalar(0,255,0));
		}
		
		//Benchmark
		time(&end);
		++counter;
		sec = difftime(end, start);
		fps = counter/sec;
		std::cout<<"FPS: "<<fps<<std::endl;
		// Display
		// rectangle(raw_left_frame, b_left, Scalar(255,0,0));
		// rectangle(raw_right_frame, b_right, Scalar(255,0,0));        
		// imshow("CURRENT LEFT FRAME",raw_left_frame);
		// imshow("CURRENT RIGHT FRAME",raw_right_frame);
		imshow("CROPPED LEFT FRAME",left_frame);
		imshow("CROPPED RIGHT FRAME",right_frame);  
	//	imshow("RIGHT HOMOGRAPHY", right_view);      
	//	imshow("LEFT HOMOGRAPHY", left_view);      
	//	imshow("RIGHT BIT", right_bit);
	//	imshow("LEFT BIT", left_bit);
		 imshow("OUTPUT", out_view);
		char k = (char)waitKey(30); 
		if( k == 27 ) break;
	}
  }

/**
 * @function getBestContour
 */

vector<Point> getBestContour(const Mat& img)
{
	vector<vector<Point> > contours;
	Mat temp(img);
	findContours(temp, contours, RETR_TREE, CHAIN_APPROX_SIMPLE);

	vector<Point> contours2;
	double area0 = 0;
	double area1 = 0;
	for (int i=0; i<contours.size(); i++) {
		area1 = fabs(contourArea(contours[i]));
		if (area1>area0) {
			area0 = area1;
			contours2 = contours[i];
		}
	}
	return contours2;
}

/**
 * @function getFloatHull
 */

vector<Point2f> getFloatHull(const vector<Point> &in , Size s, Rect bound)
{
	vector<int> hull;
	vector<Point2f> out;	
	vector<Point2f> filtered_out;
	 if (in.size()>0) 
	{
		for (int i=0; i<in.size(); i++)
		{
		//	std::cout<<Point2f(in[i].x+bound.x, in[i].y+bound.y)<<std::endl;
			out.push_back(Point2f(in[i].x+bound.x,in[i].y+bound.y));
		}
	}
	// {
	// 	convexHull(in,hull);
	// 	for (int i=0; i<hull.size(); i++)
	// 	{
	// 		Point2f p(in[hull[i] ].x+bound.x, in[hull[i] ].y+bound.y);
	// 		out.push_back(p);
	// 	}
	// 	
	// 	//filter the hull
	// 	RotatedRect area = minAreaRect(out); 
	// 	int hand_width = (area.size.width)/2; 
	// 	
	// 	float min_y = out[0].y;
	// 	int index = 0;
	// 	for (int i=1; i<out.size();i++) 
	// 	// {	
	// 	// 	//Taking l1 norm
	// 	// 	if ( ( abs(out[i].x-out[i+1].x ) + abs( out[i].y-out[i+1].y ) ) > hand_width) 
	// 	// 	{
	// 	// 		filtered_out.push_back(out[i]);
	// 	// 	}
	// 	// }
	// 	{
	// 		float out_y = (float) out[i].y;
	// 		if (out_y > min_y)
	// 		{
	// 			min_y = out_y;
	// 			index = i;
	// 		}
	// 	}
	// 	filtered_out.push_back(out[index]);
	// 	
	// }
		//out = filtered_out;

	return out;

}


/**
 * @function readme
 */
void readme()
{ std::cout << " Usage: ./SURF --fn=<vid> --fn2=<vid2>" << std::endl; }
